#include "dynamixel.h"

#define ZERO_POS 2048
#define MAX_POS  4096
#define MIN_POS  0

#define ADDR_ID     7
#define ADDR_TORQ   64
#define ADDR_LED    65
#define ADDR_GPOS   116
#define ADDR_PPOS   132
#define ADDR_VOLT   144

#define SIZE_ID     1
#define SIZE_TORQ   1
#define SIZE_LED    1
#define SIZE_GPOS   4
#define SIZE_PPOS   4
#define SIZE_VOLT   2

#define DXL_ID_START 5

#define ZERO_POS	2048

uint8_t dxl_ids[TOTAL_MOTOR_NUM];
uint32_t dxl_positions[TOTAL_MOTOR_NUM];
uint8_t dxl_led_status=0;

void Dxl_Init()
{
	Dxl_Reset();
	initPort();
}

void Dxl_Reset()
{
	uint32_t i;
	for(i=0; i<TOTAL_MOTOR_NUM; i++)
	{
		dxl_positions[i] = ZERO_POS;
		dxl_ids[i] = i+DXL_ID_START;
	}
}

void Dxl_Set_Leds(uint8_t s)
{
	uint8_t i;
	groupSyncWrite(ADDR_LED, SIZE_LED);
	for(i=0; i<TOTAL_MOTOR_NUM; i++)
	{
		groupSyncWriteAddParam(i+DXL_ID_START, s, SIZE_ID);
	}
	groupSyncWriteTxPacket();
}

void Dxl_Set_Tors(uint8_t s)
{
	uint8_t i;
	groupSyncWrite(ADDR_TORQ, SIZE_TORQ);
	for(i=0; i<TOTAL_MOTOR_NUM; i++)
	{
		groupSyncWriteAddParam(i+DXL_ID_START, s, SIZE_TORQ);
	}
	groupSyncWriteTxPacket();
}

void Dxl_Set_Gpos()
{
	uint8_t i;
	groupSyncWrite(ADDR_GPOS, SIZE_GPOS);
	for(i=0; i<TOTAL_MOTOR_NUM; i++)
	{
		groupSyncWriteAddParam(dxl_ids[i], dxl_positions[i], SIZE_GPOS);
	}
	groupSyncWriteTxPacket();
}
